// ======================================================================
// Copyright (C) 2013 Hell-Prototypes. / www.hellprototypes.com
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or (at
// your option) any later version.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Public License for more details.
// ======================================================================
#include "pap_thread.h"

pap_thread::pap_thread(QObject *parent) :
    QThread(parent)
{
    m_cam_image_width = -1;
    m_cam_image_height = -1;

    m_move_speed = 5;
    m_only_pickup_part = false;

    m_picker_offset_x = 0;
    m_picker_offset_y = 0;
}

pap_thread::~pap_thread()
{

}

void pap_thread::run()
{
    /* 1: pick up */
    if(pick_up_one_part(m_move_param_part)) {
        if(!m_only_pickup_part) {
            /* 2: adjust orient */
            if(set_picker_angle(m_move_param_target_orient)) {
                /* 3: place part */
                if(place_one_part(m_move_param_place_coor)) {
                    return;
                } else {
                    emit pap_error_process(-12);
                    emit disp_system_msg(QString("Place part fail at (%1, %2)").arg(m_move_param_place_coor.x).arg(m_move_param_place_coor.y));
                }
            } else {
                emit pap_error_process(-11);
                emit disp_system_msg(QString("Adjusting part orientation fail (%1 to %2)").arg(m_move_param_part.angle).arg(m_move_param_target_orient));
            }
        }
    } else {
        emit pap_error_process(-10);
        emit disp_system_msg(QString("Part pick up fail at (%1, %2)").arg(m_move_param_part.center.x).arg(m_move_param_part.center.y));
    }

    turn_pump_on(false);
}

void pap_thread::set_move_param(part_t &part, DPoint &place_coor, double &target_orient)
{
    m_move_param_part = part;
    m_move_param_place_coor = place_coor;
    m_move_param_target_orient = target_orient;
}

bool pap_thread::pick_up_one_part(const part_t &part)
{
    iPoint part_pos = pixel_coor_to_arm_arg(part.center);
/*
    //0. move up picker if necessary
    if(!picker_move_up()) {
        disp_system_msg("Fail to move picker up");
        return false;
    }
*/
    //1. reset picker angle
    double angle = part.angle;
    if(angle < 0) {
        angle += PI;
    }
    if(!set_picker_angle(PI - angle)) {
        emit disp_system_msg("Fail to set picker angle before pickup");
        return false;
    }

    //2. turn on pump
    turn_pump_on(true);

    int backup_speed = m_move_speed;
    if(!set_move_speed(1)) {
        emit disp_system_msg("Fail set move speed 1");
        return false;
    }
    //3. move picker to target part position
    if(!move_arm_to(part_pos, 1000)) {
        emit disp_system_msg("Fail move picker to target part");
        return false;
    }
    if(!set_move_speed(backup_speed)) {
        emit disp_system_msg("Fail set move speed 1");
        return false;
    }

    //4. move down
    if(!pick_picker_move_down()) {
        emit disp_system_msg("Fail to move picker down");
        return false;
    }

    //wait for catch up
    msleep(1000);

    //5. move up picker
    if(!picker_move_up()) {
        emit disp_system_msg("Fail to move picker up");
        return false;
    }

    return true;
}

bool pap_thread::place_one_part(DPoint &place_coor)
{
    iPoint place_arg;

    if(!place_coor_to_arm_arg(&place_coor, place_arg)) {
        emit disp_system_msg("Fail to convert place coordinate to arm arg");
        return false;
    }

    if(move_arm_to( place_arg, 1000)) {
        if(!place_picker_move_down()) {
            emit disp_system_msg("Fail to move picker down");
            return false;
        }

        turn_pump_on(false);
        msleep(1000);

        if(!picker_move_up()) {
            emit disp_system_msg("Fail to move picker up");
            return false;
        }

        return true;
    } else {
        emit disp_system_msg("Fail to convert polar coordinate to ctrl param");
    }

    return false;
}

void pap_thread::set_image_size(int w, int h)
{
    m_cam_image_width = w;
    m_cam_image_height = h;
}

